import java.awt.geom.Line2D;

public class MathUtils {
	
	// Note Y in Line2D corresponds to Z position
	public static double getAngleBetween(Line2D wallVector, Line2D vehicleVector) {
		Point2D centerPoint = getIntersection(wallVector, vehicleVector);
		Point2D wallFront = new Point2D(wallVector.getX1(), wallVector.getY1());
		Point2D vehicleForward = new Point2D(vehicleVector.getX1(), vehicleVector.getY1());
		if(wallVector.getP2().getY() > wallFront.y) wallFront = new Point2D(wallVector.getX2(), wallVector.getY2());
		if(vehicleVector.getP2().getY() > vehicleForward.y) vehicleForward = new Point2D(vehicleVector.getX2(), vehicleVector.getY2());
		Point2D origin = getIntersection(wallVector, vehicleVector);
		Point2D wallFrontNormal = new Point2D(wallFront.x - origin.x, wallFront.y - origin.y);
		Point2D vehicleForwardNormal = new Point2D(vehicleForward.x - origin.x, vehicleForward.y - origin.y);
		double wallMagnitude = Math.sqrt(wallFrontNormal.x * wallFrontNormal.x + wallFrontNormal.y * wallFrontNormal.y);
		double vehicleMagnitude = Math.sqrt(vehicleForwardNormal.x * vehicleForwardNormal.x + vehicleForwardNormal.y * vehicleForwardNormal.y);
		double cosTheta = (wallFrontNormal.x * vehicleForwardNormal.x + wallFrontNormal.y * vehicleForwardNormal.y) / (wallMagnitude * vehicleMagnitude);
		return Math.acos(cosTheta);
	}
	
	public static Point2D getIntersection(Line2D l1, Line2D l2) {
		float x1 = (float) l1.getP1().getX();
		float x2 = (float) l1.getP2().getX();
		float x3 = (float) l2.getP1().getX();
		float x4 = (float) l2.getP2().getX();
		float y1 = (float) l1.getP1().getY();
		float y2 = (float) l1.getP2().getY();
		float y3 = (float) l2.getP1().getY();
		float y4 = (float) l2.getP2().getY();	
		float x = (x1 * y2 - y1 * x2) * (x3 - x4) - (x1 - x2) * (x3 * y4 - y3 * x4);
		float y = (x1 * y2 - y1 * x2) * (y3 - y4) - (y1 - y2) * (x3 * y4 - y3 * x4);
		float denom = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
		// If denom == 0 lines are parallel
		if(denom == 0) return new Point2D(l1.getP1().getX(), l1.getP1().getY());
		return new Point2D(x / denom, y / denom);
		
	}
	
}